#include "WPILib.h"

#ifndef ROBOT_H_
#define ROBOT_H_

//xbox Digital Controls
#define XBOX_A          1
#define XBOX_B          2
#define XBOX_X          3
#define XBOX_Y          4
#define XBOX_LBUMP      5
#define XBOX_RBUMP      6
#define XBOX_BACK       7
#define XBOX_START      8
#define XBOX_LSTICK     9
#define XBOX_RSTICK     10

//xbox Analog Controls
#define XBOX_LSTICK_X   1
#define XBOX_LSTICK_Y   2
#define XBOX_TRIGGERS   3       //-1 = right; +1 = left
#define XBOX_RSTICK_X   4
#define XBOX_RSTICK_Y   5

#define JOYSTICKDEADSPACE       0.1

//usb camera ports
#define topView "cam1"
#define midView "cam0"

//Defines how much the robot will correct during autonomous
#define correctConstant .25
#define correctSensitivity 100 //400 = 1 Rotation?
#define autonDistance 36050

//joystick deadspace
#define JOYSTICKDEADSPACE       0.1

	class Robot : public IterativeRobot
	{


		CANTalon driveFLeft;
		CANTalon driveFRight;
		CANTalon driveBLeft;
		CANTalon driveBRight;
		CANTalon liftMotor;

		RobotDrive myRobot; // robot drive system

		Joystick rightStick; // right stick
		Joystick leftStick; // left stick
		Joystick xbox; // xbox controller
		Joystick launch; //launchpad board

		Compressor comp;
		Solenoid shift;
		Solenoid shiftRetract;
		Solenoid leftArm;
		Solenoid leftArmRetract;
		Solenoid rightArm;
		Solenoid rightArmRetract;

		DigitalInput liftSensor;
		DigitalInput autonMode1;
		DigitalInput autonMode2;

		Encoder liftEncoder;
		Encoder leftDrive;
		Encoder rightDrive;

		Gyro gyro;

		bool rightOn;
		bool rightPressed;

		bool leftOn;
		bool leftPressed;

		bool isInFirstGear;

		bool cam1on;
		bool camTogglePressed;

		int presetMode;

		//bool firstTime;

		//auton variables
		int autonMode;

		bool autonDSDone;

		int autonPPStage;
		bool autonPPDone;
		int autonPPCount;

		int autonPDStage;
		bool autonPDDone;

		int autonNSStage;
		bool autonNSDone;

		int teleopCount;
		int autonCount;

		double lspeed;
		double rspeed;

		IMAQdxSession session;
		Image *frame;
		IMAQdxError imaqError;

	public:
		Robot();

		void RobotInit();

		void AutonomousInit();
		void AutonomousPeriodic();

		void TeleopInit();
		void TeleopPeriodic();

		void TestInit();
		void TestPeriodic();

		void DisabledInit();
		void DisabledPeriodic ();

		void setRightSide(double speed);
		void setLeftSide(double speed);

		void autonDriveStraight();
		void autonPickUpAndPush();
		void autonPickUpAndDrive();
		void autonNemesisStrat();
	};



#endif
